#include <cstdio>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/video.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace cv;


#include "targettrack.h"

TargetTrack::TargetTrack():m_track_imgs(40)
{
    sem_init(&this->m_frame_mutex, 0, 1);
    sem_init(&this->m_track_mutex, 0, 1);
    this->m_status = false;
    this->m_track_new = true;
}

TargetTrack::~TargetTrack(){

    printf("track closed\t");
    TimeStamp::now()->print();
}

bool TargetTrack::getImage(Mat &img, TimeStamp &ts){
    sem_wait(&this->m_frame_mutex);
    if(!this->m_cur.empty()){
        this->m_cur.copyTo(img);
        ts = this->m_cur_ts;
        sem_post(&this->m_frame_mutex);
        return true;
    }else{
        sem_post(&this->m_frame_mutex);
        return false;
    }

}

void TargetTrack::setCamera(CameraDaemon* cam){
    this->m_cam = cam;
}

//void TargetTrack::nextFrame(){
//    Mat temp;
//    TimeStamp temp_ts;
//    sem_wait(&this->m_frame_mutex);
//    if(this->m_cam->getImage(temp, temp_ts)){
//        if(!this->m_cur.empty()){
//            this->m_cur.copyTo(this->m_prev);
//            this->m_prev_ts = this->m_cur_ts;
//        }
//        temp.copyTo(this->m_cur);
//        this->m_cur_ts = temp_ts;
//    }
//    sem_post(&this->m_frame_mutex);

//    if(!temp.empty()){
//        sem_wait(&this->m_track_mutex);
//        if(this->m_status){
//            if(this->m_track_new){
//                this->m_track_new = false;
//                calcOpticalFlowPyrLK(this->m_track_img,
//                                     temp,
//                                     this->m_track_keypoints,
//                                     this->m_cur_keypoints,
//                                     this->m_cur_keypoints_status,
//                                     this->m_cur_keypoints_err);
//                if(this->m_cur_keypoints.empty()){
//                    this->m_cur_keypoints = this->m_track_keypoints;
//                    fprintf(stderr, "can't find at first\n");
//                }
//            }else{
//                vector<Point2f> prev_keypoints(this->m_cur_keypoints);
//                this->m_cur_keypoints.clear();
//                calcOpticalFlowPyrLK(this->m_prev,
//                                     this->m_cur,
//                                     prev_keypoints,
//                                     this->m_cur_keypoints,
//                                     this->m_cur_keypoints_status,
//                                     this->m_cur_keypoints_err);
//                if(this->m_cur_keypoints.empty()){
//                    this->m_cur_keypoints = prev_keypoints;
//                    fprintf(stderr, "can't find in the progress\n");
//                }
//            }
//        }
//        sem_post(&this->m_track_mutex);
//    }



//}

void TargetTrack::nextFrame()
{
    Mat temp;
    TimeStamp temp_ts;
    sem_wait(&this->m_frame_mutex);
    if(this->m_cam->getImage(temp, temp_ts)){
        if(!this->m_cur.empty()){
            this->m_cur.copyTo(this->m_prev);
            this->m_prev_ts = this->m_cur_ts;
        }
        temp.copyTo(this->m_cur);
        this->m_cur_ts = temp_ts;
    }
    sem_post(&this->m_frame_mutex);

    if(!temp.empty()){
          Mat mask;
          sem_wait(&this->m_track_mutex);
          if(this->m_status){
              if (this->m_track_new){
                  if (this->m_motion_detector.shakeAdjust(
                              this->m_track_img,
                              this->m_cur,
                              Rect(0, 0, this->m_track_img.cols, this->m_track_img.rows),
                              this->m_search_area,
                              mask)){
                    selectMask(mask, this->m_search_area, mask, &this->m_obj_center);
                    if (this->m_feature_matcher.detectOrb(this->m_cur, mask, TimeStamp())){
                        this->m_velocity_x = 0;
                        this->m_velocity_y = 0;
                    }
                  }
                  this->m_track_new = false;
              }else{
                  if (this->m_motion_detector.shakeAdjust(this->m_prev,
                                                      this->m_cur,
                                                      Rect(0, 0, this->m_track_img.cols, this->m_track_img.rows),
                                                      Rect(this->m_top_left, this->m_bottom_right),
                                                      mask)){

                      motionEstimate(this->m_prev_ts, this->m_cur_ts, this->m_search_area, this->m_search_area);
                      selectMask(mask, this->m_search_area, mask, NULL);
                      if (this->m_feature_matcher.matchOrb(this->m_cur, mask, TimeStamp())){
//                          imshow("match", this->m_feature_matcher.m_match_img);
                      }
                      updateMotionInfo(this->m_feature_matcher.m_matched_points, this->m_prev_ts, this->m_cur_ts);
                  }
              }
          }
          sem_post(&this->m_track_mutex);
      }


}

static void getMidPoint(Point2f pt1, Point2f pt2, Point2f &ptm){
    ptm.x = pt1.x + pt2.x;
    ptm.y = pt1.y + pt2.y;
    ptm.x /= 2;
    ptm.y /= 2;
}

bool TargetTrack::findSiftKeypoints(Mat &obj, vector<Point2f> &keypoints, int &count){
    SIFT sift;
    vector<KeyPoint> keypoints_obj;
    sift(obj, Mat::ones(obj.cols, obj.rows, CV_8U), keypoints_obj, cv::noArray());
    for(unsigned int i = 0; i < keypoints_obj.size(); i++){
        keypoints.push_back(keypoints_obj[i].pt);
    }
    if(keypoints.size() >= (unsigned int)count){
        count = keypoints.size();
        return true;
    }else{
        count = keypoints.size();
        return false;
    }
}

static void shiftKeypoints(vector<Point2f> &keypoints, Point2f top_left){
    for(unsigned int i = 0; i < keypoints.size(); ++i){
        keypoints[i].x += top_left.x;
        keypoints[i].y += top_left.y;
    }
}




void TargetTrack::setTarget(Point2f top_left, Point2f bottom_right){
    Mat roi = this->m_track_img(Rect(top_left, bottom_right));
    int count = 1;

    if(!findSiftKeypoints(roi, this->m_track_keypoints, count)){
//        printf("keypoints couthis->m_track_imgnt = %d\n", count);
        Point2f middle;
        getMidPoint(top_left, bottom_right, middle);
        this->m_track_keypoints.push_back(middle);
    }
//    shiftKeypoints(this->m_track_keypoints, top_left);
}

bool TargetTrack::getTrackImage(Mat &img, TimeStamp &ts){
    sem_wait(&this->m_track_mutex);
    bool ret;
    ret = this->getImage(img, ts);
    if(ret){
        this->m_track_imgs.enqueue(ts.toInt(), img);
     }

    sem_post(&this->m_track_mutex);

    return ret;
}

static void drawTarget(Mat &img, Point2f center, Rect area){
    Scalar blue(255, 0 , 0);
    Scalar red(0, 0, 255);
    Scalar green(0, 255, 0);
    circle(img, center, 3, red, 2);
    rectangle(img, area, blue, 2);
    printf("(%3f,%3f)\n", center.x, center.y);
}

bool TargetTrack::getTrackResult(Mat &img){
    bool ret;
    sem_wait(&this->m_frame_mutex);
    ret = !this->m_cur.empty();
    if(ret){
        this->m_cur.copyTo(img);
        if(this->m_status){
            drawTarget(img, this->m_obj_center, this->m_search_area) ;
        }
    }
    sem_post(&this->m_frame_mutex);
    return ret;
}

void TargetTrack::getTrackResult(TimeStamp &ts, Rect &area, Point2f &center){
    ts = this->m_cur_ts;
    area = this->m_search_area;
    center = this->m_obj_center;
}



void TargetTrack::startTarck(TimeStamp &ts, int x1, int y1, int x2, int y2){
    sem_wait(&this->m_track_mutex);


    printf("req\t");
    ts.print();
    printf("prev\t");
    this->m_prev_ts.print();

    if(this->m_track_imgs.find(ts.toInt(), &this->m_track_img)){
        this->m_track_img_ts = ts;
        printf("found img\n");
        this->m_cur_keypoints.clear();
        this->m_track_new = true;
        this->m_top_left = Point2f(x1, y1);
        this->m_bottom_right = Point2f(x2, y2);
        this->m_search_area = Rect(this->m_top_left, this->m_bottom_right);
        getMidPoint(this->m_top_left, this->m_bottom_right, this->m_obj_center);
        this->m_velocity_x = 0;
        this->m_velocity_y = 0;
//        this->setTarget(this->m_top_left, this->m_bottom_right);
        this->m_status = true;
    }else{

    }

//    if(this->m_track_img_ts == ts){
//        this->m_cur_keypoints.clear();
//        this->m_track_new = true;
//        this->m_top_left = Point2f(x1, y1);
//        this->m_bottom_right = Point2f(x2, y2);
//        this->setTarget(this->m_top_left, this->m_bottom_right);
//        this->m_status = true;
//    }else{


////        this->m_track_new = true;
////        this->m_top_left = Point2f(x1, y1);
////        this->m_bottom_right = Point2f(x2, y2);
////        this->setTarget(this->m_top_left, this->m_bottom_right);
////        this->m_status = true;
//    }
    sem_post(&this->m_track_mutex);
}

bool TargetTrack::selectMask(Mat mask, Rect area, Mat& out, Point2f* center){
    Mat temp = Mat::zeros(mask.size(), CV_8U);
    temp(area).setTo(255);
    bitwise_and(mask, temp, out);
    Moments m = moments(out, true);
    if(m.m00 < 1){
        return false;
    }else{
        if(center != NULL){
            center->x = m.m10 / m.m00;
            center->y = m.m01 / m.m00;
        }
    }
    return true;
}

static inline void setRectRange(Rect &rect, int width, int height){
    if (rect.x < 0 ){
        rect.x = 0;
    }else if (rect.x > width - 1){
        rect.x = width - 1;
    }
    if (rect.y < 0){
        rect.y = 0;
    }else if (rect.y > height - 1){
        rect.y = height - 1;
    }
    if (rect.width <= 0){
        rect.width = 1;
    }else if(rect.x + rect.width > width){
        rect.width = width - rect.x;
    }
    if(rect.height <= 0){
        rect.height = 1;
    }else if(rect.height + rect.y > height){
        rect.height = height - rect.y;
    }
}

void TargetTrack::motionEstimate(TimeStamp last_ts,
                                 TimeStamp cur_ts,
                                 Rect last_area,
                                 Rect &estimated_area){
    int time = cur_ts.diff(last_ts);
    double est_disp_x = this->m_velocity_x * time;
    double est_disp_y = this->m_velocity_y * time;
    estimated_area = last_area;
    if(abs(est_disp_x) >= last_area.width / 2){
        estimated_area.width *= 2;
    }
    if(abs(est_disp_y) >= last_area.height / 2){
        estimated_area.height *= 2;
    }
    estimated_area.x += est_disp_x;
    estimated_area.y += est_disp_y;
    setRectRange(estimated_area, this->m_cur.cols, this->m_cur.rows);
}

void TargetTrack::updateMotionInfo(vector<KeyPoint> points, TimeStamp last_ts, TimeStamp cur_ts){
    int num = -1;
    if(points.size() > 0){
        double min_dist = this->m_cur.cols + this->m_cur.rows;
        for (int i = 0; i < (int)points.size(); i++){
            double dx = points[i].pt.x - this->m_obj_center.x;
            double dy = points[i].pt.y - this->m_obj_center.y;
            double dist = sqrt(dx * dx + dy * dy);
            if (dist < min_dist){
                min_dist = dist;
                num = i;
            }
        }
        Point2f center = points[num].pt;
        double disp_x, disp_y;
        int time = cur_ts.diff(last_ts);
        disp_x = center.x - this->m_obj_center.x;
        disp_y = center.y - this->m_obj_center.y;
        double v_x = disp_x / time;
        double v_y = disp_y / time;
        this->m_velocity_x = this->m_velocity_x * 0.5 + v_x * 0.5;
        this->m_velocity_y = this->m_velocity_y * 0.5 + v_y * 0.5;
        this->m_obj_center = center;
        this->m_search_area.x = center.x - this->m_search_area.width / 2;
        this->m_search_area.y = center.y - this->m_search_area.height / 2;
        setRectRange(this->m_search_area, this->m_cur.cols, this->m_cur.rows);
    }
}
